#include "motor_pid.h"
#include "bsp_can.h"
#include "bsp_pid_controller.h"

// ==================== 电机PID参数配置区 ====================
// 调试时主要修改这些参数

// 速度环PID参数 - 调试时按顺序调整：先P，再I，最后D
static const float VELOCITY_PID_KP = 0.8f;   // 速度环P: 增大使速度响应更快
static const float VELOCITY_PID_KI = 0.2f;   // 速度环I: 消除速度静差
static const float VELOCITY_PID_KD = 0.02f;  // 速度环D: 抑制速度超调

// 位置环PID参数 - 位置控制时使用
static const float POSITION_PID_KP = 5.0f;   // 位置环P: 主要参数，决定位置刚度
static const float POSITION_PID_KI = 0.5f;   // 位置环I: 消除位置静差
static const float POSITION_PID_KD = 0.1f;   // 位置环D: 抑制位置超调

// PID输出限幅 - 根据电机最大电流调整
static const float PID_MAX_CURRENT = 10000.0f;  // 最大电流输出
static const float PID_MAX_INTEGRAL = 5000.0f;  // 最大积分项
// ========================================================

// PID控制器实例
static pid_controller_t velocity_pids[MOTOR_COUNT];
static pid_controller_t position_pids[MOTOR_COUNT];

// 控制模式
static pid_control_mode_t control_mode = CONTROL_MODE_VELOCITY;

/**
 * @brief 初始化电机PID控制器
 */
void motor_pid_init(void)
{
    float control_dt = 0.001f;  // 1ms控制周期，根据实际情况调整
    
    for (int i = 0; i < MOTOR_COUNT; i++) {
        // 初始化速度环PID（增量式）
        pid_init(&velocity_pids[i], PID_MODE_VELOCITY, control_dt);
        pid_set_parameters(&velocity_pids[i], VELOCITY_PID_KP, VELOCITY_PID_KI, VELOCITY_PID_KD);
        pid_set_limits(&velocity_pids[i], PID_MAX_CURRENT, PID_MAX_INTEGRAL);
        
        // 初始化位置环PID（位置式）
        pid_init(&position_pids[i], PID_MODE_POSITION, control_dt);
        pid_set_parameters(&position_pids[i], POSITION_PID_KP, POSITION_PID_KI, POSITION_PID_KD);
        pid_set_limits(&position_pids[i], PID_MAX_CURRENT, PID_MAX_INTEGRAL);
    }
}

/**
 * @brief 设置控制模式
 */
void motor_pid_set_mode(pid_control_mode_t mode)
{
    control_mode = mode;
}

/**
 * @brief 速度控制
 * @param motor: 电机ID
 * @param target_rpm: 目标速度(RPM)
 * @return 计算出的电流指令
 */
int16_t motor_pid_velocity_control(motor_id_t motor, float target_rpm)
{
    if (motor >= MOTOR_COUNT) return 0;
    
    // 获取当前速度反馈
    motor_feedback_t *fb = bsp_can_get_motor_feedback_ptr(motor);
    if (fb == NULL || !fb->connected) return 0;
    
    // PID计算
    float current_output = pid_calculate(&velocity_pids[motor], target_rpm, fb->velocity_rpm);
    
    return (int16_t)current_output;
}

/**
 * @brief 位置控制
 * @param motor: 电机ID  
 * @param target_deg: 目标角度(度)
 * @return 计算出的电流指令
 */
int16_t motor_pid_position_control(motor_id_t motor, float target_deg)
{
    if (motor >= MOTOR_COUNT) return 0;
    
    // 获取当前位置反馈
    motor_feedback_t *fb = bsp_can_get_motor_feedback_ptr(motor);
    if (fb == NULL || !fb->connected) return 0;
    
    // PID计算
    float current_output = pid_calculate(&position_pids[motor], target_deg, fb->total_angle_deg);
    
    return (int16_t)current_output;
}

/**
 * @brief 所有电机速度控制
 */
void motor_pid_all_velocity_control(float rpm_a, float rpm_b, float rpm_c, float rpm_d)
{
    int16_t current_a = motor_pid_velocity_control(MOTOR_A, rpm_a);
    int16_t current_b = motor_pid_velocity_control(MOTOR_B, rpm_b);
    int16_t current_c = motor_pid_velocity_control(MOTOR_C, rpm_c);
    int16_t current_d = motor_pid_velocity_control(MOTOR_D, rpm_d);
    
    bsp_can_send_all_motors_current(current_a, current_b, current_c, current_d);
}

/**
 * @brief 所有电机位置控制
 */
void motor_pid_all_position_control(float deg_a, float deg_b, float deg_c, float deg_d)
{
    int16_t current_a = motor_pid_position_control(MOTOR_A, deg_a);
    int16_t current_b = motor_pid_position_control(MOTOR_B, deg_b);
    int16_t current_c = motor_pid_position_control(MOTOR_C, deg_c);
    int16_t current_d = motor_pid_position_control(MOTOR_D, deg_d);
    
    bsp_can_send_all_motors_current(current_a, current_b, current_c, current_d);
}

/**
 * @brief 麦轮速度控制（带PID）
 */
void motor_pid_mecanum_velocity(float vx, float vy, float vz)
{
    // 麦轮运动学分解
    float wheel_a = -vx + vy + vz;
    float wheel_b = vx + vy + vz;
    float wheel_c = vx + vy - vz;
    float wheel_d = -vx + vy - vz;
    
    motor_pid_all_velocity_control(wheel_a, wheel_b, wheel_c, wheel_d);
}

/**
 * @brief 重置所有PID控制器
 */
void motor_pid_reset_all(void)
{
    for (int i = 0; i < MOTOR_COUNT; i++) {
        pid_reset(&velocity_pids[i]);
        pid_reset(&position_pids[i]);
    }
}

/**
 * @brief 获取PID调试信息
 */
void motor_pid_get_debug_info(motor_id_t motor, pid_debug_info_t *info)
{
    if (motor >= MOTOR_COUNT || info == NULL) return;
    
    pid_controller_t *pid = (control_mode == CONTROL_MODE_VELOCITY) ? 
                           &velocity_pids[motor] : &position_pids[motor];
    
    info->output = pid_get_last_output(pid);
    info->error = pid_get_last_error(pid);
    pid_get_debug_info(pid, &info->p_term, &info->i_term, &info->d_term);
    info->update_count = pid->update_count;
}

/**
 * @brief 设置单个电机速度环PID参数 - 调试时使用
 */
void motor_pid_set_velocity_params(motor_id_t motor, float kp, float ki, float kd)
{
    if (motor < MOTOR_COUNT) {
        pid_set_parameters(&velocity_pids[motor], kp, ki, kd);
    }
}

/**
 * @brief 设置单个电机位置环PID参数 - 调试时使用
 */
void motor_pid_set_position_params(motor_id_t motor, float kp, float ki, float kd)
{
    if (motor < MOTOR_COUNT) {
        pid_set_parameters(&position_pids[motor], kp, ki, kd);
    }
}

/**
 * @brief 设置所有电机速度环PID参数 - 调试时使用
 */
void motor_pid_set_all_velocity_params(float kp, float ki, float kd)
{
    for (int i = 0; i < MOTOR_COUNT; i++) {
        pid_set_parameters(&velocity_pids[i], kp, ki, kd);
    }
}

/**
 * @brief 设置所有电机位置环PID参数 - 调试时使用
 */
void motor_pid_set_all_position_params(float kp, float ki, float kd)
{
    for (int i = 0; i < MOTOR_COUNT; i++) {
        pid_set_parameters(&position_pids[i], kp, ki, kd);
    }
}

/**
 * @brief 获取速度环PID参数
 */
void motor_pid_get_velocity_params(motor_id_t motor, float *kp, float *ki, float *kd)
{
    if (motor < MOTOR_COUNT) {
        if (kp) *kp = velocity_pids[motor].kp;
        if (ki) *ki = velocity_pids[motor].ki;
        if (kd) *kd = velocity_pids[motor].kd;
    }
}

/**
 * @brief 获取位置环PID参数
 */
void motor_pid_get_position_params(motor_id_t motor, float *kp, float *ki, float *kd)
{
    if (motor < MOTOR_COUNT) {
        if (kp) *kp = position_pids[motor].kp;
        if (ki) *ki = position_pids[motor].ki;
        if (kd) *kd = position_pids[motor].kd;
    }
}